#!/usr/bin/env python

import rospy
import sys
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import LaserScan
from laser_geometry import LaserProjection

class Laser2PC():
    def __init__(self, robot, num):
        self.robot = robot
        self.num = num
        self.laserProj = LaserProjection()
        self.pcPub = [None] * self.num
        self.laserSub = [None] * self.num
        for i in range(self.num):
            print(robot+"_"+str(i)+"/scan")
            self.pcPub[i] = rospy.Publisher(robot+"_"+str(i)+"/laserPointCloud", PointCloud2, queue_size=1)
            self.laserSub[i] = rospy.Subscriber(robot+"_"+str(i)+"/scan", LaserScan, self.laserCallback, i)

    def laserCallback(self, data, i):

        cloud_out = self.laserProj.projectLaser(data)

        self.pcPub[i].publish(cloud_out)
        print(f"publish {i}")

if __name__ == '__main__':
    rospy.init_node("laser2PointCloud")
    l2pc = Laser2PC(sys.argv[1], int(sys.argv[2]))
    rospy.spin()
